#include <Eigen/Core>
#include <opencv2/core/eigen.hpp>
#include <opencv2/calib3d.hpp>
#include <iostream>
#include <Eigen/Dense>
#include <ctime>
#include <cstdlib>
#include <math.h>
#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc/imgproc_c.h"
#include <fstream>
#include<iomanip>
#include "LightDescriptor.hpp"
//#include "Person.h"


using namespace cv;
using namespace std;
using namespace Eigen;

//包含灯条匹配，PnP解算，作图等主要功能的实现
class MatchLights
{
public:
    //MatchLights() {};
    MatchLights(const LightDescriptor& light1,const LightDescriptor& light2)
    {
        double pi=3.14159265;
        center=(light1.center+light2.center)/2;
        A.x = light1.center.x+sin(light1.angle/180*pi)*light1.length/2;
        A.y=light1.center.y-cos(light1.angle/180*pi)*light1.length/2;
        B.x=light2.center.x+sin(light2.angle/180*pi)*light2.length/2;
        B.y=light2.center.y-cos(light2.angle/180*pi)*light2.length/2;
        C=(light2.center+light2.center)-B;
        D=(light1.center+light1.center)-A;

        //打分项
        qx=(light1.center.y-light2.center.y)/(light1.center.x-light2.center.x);
        px=abs((A.y-B.y)/(A.x-B.x)-qx)+abs((D.y-C.y)/(D.x-C.x)-qx);
        ratio=2*abs(light1.center.x-light2.center.x)/(light1.length+light2.length);
        score=2*abs(qx)+px+ratio;

        //四个点的方位
        if(light1.center.x<light2.center.x && A.y<D.y){situation=1;}
        if(light1.center.x<light2.center.x && A.y>D.y){situation=2;}
        if(light1.center.x>light2.center.x && A.y<D.y){situation=3;}
        if(light1.center.x>light2.center.x && A.y>D.y){situation=4;}

    }
    const MatchLights& operator =(const MatchLights& ld)
    {
        this->center = ld.center;
        this->A = ld.A;
        this->B = ld.B;
        this->C = ld.C;
        this->D = ld.D;
        this->qx=ld.qx;
        this->px=ld.px;
        this->ratio=ld.ratio;
        this->score=ld.score;
        this->situation=ld.situation;
        return *this;
    }



    void draw(Mat img){
        line(img,A,B,Scalar(0,255,0),4);
        line(img,B,C,Scalar(0,255,0),4);
        line(img,C,D,Scalar(0,255,0),4);
        line(img,D,A,Scalar(0,255,0),4);
    }

    void GetFourPoints(vector<Point2f> &fourPoints){
        
        fourPoints.push_back(A);
        fourPoints.push_back(B);
        fourPoints.push_back(C);
        fourPoints.push_back(D);
    }

    Eigen::Vector3f PNP(){
        vector<Point3f> orig;
        vector<Point2f> xszb;
        Mat IntrinsicMatrix = (Mat_<float>(3,3)<<1822.7,0,741.4,0,1830.1,555.2,0,0,1);
        Mat coe=(Mat_<float>(4,1)<<-0.0727,0.0682,0,0);
        Mat rvec,tvec,rotMat;
        orig.push_back(Point3f(0,-67.5,34));
        orig.push_back(Point3f(0,67.5,34));
        orig.push_back(Point3f(0,67.5,-34));
        orig.push_back(Point3f(0,-67.5,-34));
        if(situation==1){
            xszb.push_back(A);
            xszb.push_back(B);
            xszb.push_back(C);
            xszb.push_back(D);
        }
        if(situation==2){
            xszb.push_back(D);
            xszb.push_back(C);
            xszb.push_back(B);
            xszb.push_back(A);
        }
        if(situation==3){
            xszb.push_back(B);
            xszb.push_back(A);
            xszb.push_back(D);
            xszb.push_back(C);
        }
        if(situation==4){
            xszb.push_back(C);
            xszb.push_back(D);
            xszb.push_back(B);
            xszb.push_back(A);
        }
        solvePnP(orig,xszb,IntrinsicMatrix,coe,rvec,tvec);
        Rodrigues(rvec, rotMat);

        Eigen::Matrix3f R_n;
	    Eigen::Vector3f T_n;
	    cv2eigen(rotMat, R_n);
	    cv2eigen(tvec, T_n);
        return -R_n.inverse()*T_n;
    }

public:
    Point2f A;
    Point2f B;
    Point2f C;
    Point2f D;
    Point2f center;
    double qx;
    double px;
    double ratio;
    double score;
    int situation;
    
};